
#include <BPointRestraint>
#include "member_BPointRestraint.h"
#include "member_BRestraint.h"
#include "member_BBody.h"
#include "member_BNode.h"

using namespace BWE;

#define member					(*(member_BPointRestraint*)_ptr)
#define member_allocate()		_ptr = new member_BPointRestraint(this)
#define member_release()		delete (member_BPointRestraint*)_ptr

BPointRestraint::BPointRestraint()
{
	member_allocate();
}
BPointRestraint::BPointRestraint(BBody* bodyA, BBody* bodyB) : BRestraint(bodyA, bodyB)
{
	member_allocate();
}
BPointRestraint::BPointRestraint(BBody* bodyA, const BVector& pivotA) : BRestraint(bodyA, pivotA)
{
	member_allocate();
}
BPointRestraint::BPointRestraint(BBody* bodyA, const BVector& pivotA, BBody* bodyB, const BVector& pivotB) : BRestraint(bodyA, pivotA, bodyB, pivotB)
{
	member_allocate();
}
BPointRestraint::~BPointRestraint()
{
	member_release();
}

void BPointRestraint::copy(const BRestraint* other)
{
	BRestraint::copy(other);
	member.impulseLimit = point_member(other)->impulseLimit;
}
BRestraint* BPointRestraint::clone() const
{
	BPointRestraint* restraint = new BPointRestraint();
	restraint->copy(this);
	return restraint;
}

void BPointRestraint::setDamping(BReal damping)
{
	member.damping = damping;
	this->emit(Signal_Changed);
}
BReal BPointRestraint::damping() const
{
	return member.damping;
}

void BPointRestraint::setImpulseLimit(BReal impulseLimit)
{
	member.impulseLimit = impulseLimit;
	this->emit(Signal_Changed);
}
BReal BPointRestraint::impulseLimit() const
{
	return member.impulseLimit;
}

int BPointRestraint::solverSize()
{
	return 3;
}
void BPointRestraint::soverItems(BSolverItem* solverItems, BReal stepTime)
{
	const BBody* bodyA = restraint_member(this)->bodyA;
	const BBody* bodyB = restraint_member(this)->bodyB;

	const BMatrix& matrixA = node_member(bodyA)->matrix;
	const BMatrix& matrixB = node_member(bodyB)->matrix;

	solverItems[0].contactNormalA.x() = 1;
	solverItems[1].contactNormalA.y() = 1;
	solverItems[2].contactNormalA.z() = 1;
	BVector vecA = matrixA.mult3(restraint_member(this)->pivotA);
	{
		BVector& torqueAxis0 = solverItems[0].torqueAxisA;
		BVector& torqueAxis1 = solverItems[1].torqueAxisA;
		BVector& torqueAxis2 = solverItems[2].torqueAxisA;
		SkewSymmetric(-vecA, torqueAxis0, torqueAxis1, torqueAxis2);
	}

	solverItems[0].contactNormalB.x() = -1;
	solverItems[1].contactNormalB.y() = -1;
	solverItems[2].contactNormalB.z() = -1;
	BVector vecB = matrixB.mult3(restraint_member(this)->pivotB);
	{
		BVector& torqueAxis0 = solverItems[0].torqueAxisB;
		BVector& torqueAxis1 = solverItems[1].torqueAxisB;
		BVector& torqueAxis2 = solverItems[2].torqueAxisB;
		SkewSymmetric(vecB, torqueAxis0, torqueAxis1, torqueAxis2);
	}

	BVector pointA = matrixA * restraint_member(this)->pivotA;
	BVector pointB = matrixB * restraint_member(this)->pivotB;
	BVector offset = pointB - pointA;

	solverItems[0].velocity = offset.x() / stepTime;
	solverItems[1].velocity = offset.y() / stepTime;
	solverItems[2].velocity = offset.z() / stepTime;

	member.velocityError.x() = solverItems[0].velocity;
	member.velocityError.y() = solverItems[1].velocity;
	member.velocityError.z() = solverItems[2].velocity;

	if (member.impulseLimit > 0)
	{
		solverItems[0].impulseLowerLimit = -member.impulseLimit;
		solverItems[0].impulseUpperLimit = member.impulseLimit;

		solverItems[1].impulseLowerLimit = -member.impulseLimit;
		solverItems[1].impulseUpperLimit = member.impulseLimit;

		solverItems[2].impulseLowerLimit = -member.impulseLimit;
		solverItems[2].impulseUpperLimit = member.impulseLimit;
	}

}

bool BPointRestraint::query(const BString& text, BValue& value) const
{
	if (text == "velocity-error")
	{
		value = member.velocityError;
		return true;
	}
	return BRestraint::query(text, value);
}
